/*
 * signal_toolkit.cxx
 *
 *  Created on: Oct 21, 2011
 *      Author: Bernhard Schildendorfer
 */

#include "crypto/signal_toolkit.h"
#include "logger.h"
#include <cstdlib>
#include <cmath>

CSignalToolkit::CSignalToolkit(){
}

CSignalToolkit::CSignalToolkit(unsigned int pSampleRate){
	printl(0, __FILE__, "constructing CSignalToolkit");
	mSampleRate=pSampleRate;
}

CSignalToolkit::~CSignalToolkit() {

}




float CSignalToolkit::findFrequencyPattern(
		double* pSamples,
		unsigned int pSampleCount,
		unsigned int* pFrequencies,
		unsigned int pFrequencyCount){
	return findFrequencyPattern(pSamples, pSampleCount, pFrequencies, pFrequencyCount,-1);
}





float CSignalToolkit::findFrequencyPattern(
		double* pSamples,
		unsigned int pSampleCount,
		unsigned int* pFrequencies,
		unsigned int pFrequencyCount,
		double pMaxAmplitude){


	float lMinAmplitude=100, lMaxAmplitude=0;
	float amplitude;
	fftw_complex* lTimeDomain        = ( fftw_complex* ) fftw_malloc( sizeof( fftw_complex ) * pSampleCount );
	fftw_complex* lFrequencyDomain  = ( fftw_complex* ) fftw_malloc( sizeof( fftw_complex ) * pSampleCount );
	fftw_plan lFFT  = fftw_plan_dft_1d( pSampleCount, lTimeDomain, lFrequencyDomain, FFTW_FORWARD, FFTW_ESTIMATE );

	 // populate input data
	for(unsigned int i = 0 ; i < pSampleCount ; i++ ) {
		lTimeDomain[i][0] = pSamples[i];
		lTimeDomain[i][1] = 0.0;
	}

	fftw_execute(lFFT);

	if(pMaxAmplitude==-1){
		//the three highest amplitude values lH1>lH2>lH3
		float lH1=0,lH2=0,lH3=0;
		//find the maximum and minimum frequency value
		for(unsigned int i=0;i<pSampleCount/2;i++){
			//calculate the amplitude of the complex datatype
			amplitude = getAmplitudeOfComplex(lFrequencyDomain[i]);
			if(amplitude<lMinAmplitude)
				lMinAmplitude=amplitude;
			if(amplitude>lH3){
				if(amplitude>lH1){
					lH3=lH2;
					lH2=lH1;
					lH1=amplitude;
				}else{
					if(amplitude>lH2){
						lH3=lH2;
						lH2=amplitude;
					}else{
						lH3=amplitude;
					}
				}
			}
		}
			//smooth frequency peaks a little bit
		lMaxAmplitude=(lH1+lH2+lH3)/3;
	}else{
		lMaxAmplitude=pMaxAmplitude;
	}


	float baseFrequency = mSampleRate / (float)pSampleCount;
	bool lIsSyncSignal=false;	//in the loop we proof its not the signal
	int index;
	float lScore=0;

	//The maximum frequency must at least be 5 to start looking for patterns
	//the minimal value should be less than the half of max for recognition
	if(lMaxAmplitude>5 && lMinAmplitude<lMaxAmplitude*0.8){
		//assume it is the sync signal and proof to be false
		lIsSyncSignal=true;
		// look for the given frequencies in the signal
		for (unsigned int i=0; i < pFrequencyCount ; i++) {
			index = pFrequencies[i] / baseFrequency;
			lScore+=getAmplitudeOfComplex(lFrequencyDomain[index])/lMaxAmplitude;
		}
		lScore/=pFrequencyCount;
	}

	//Clean up
	fftw_destroy_plan( lFFT );
	fftw_free(lTimeDomain);
	fftw_free(lFrequencyDomain);

	if(lScore>1)
		return 1;
	else
		return lScore;
}


unsigned int CSignalToolkit::findFrequencyPatternPosition(
		double* pSamples,
		unsigned int pSampleCount,
		unsigned int* pFrequencies,
		unsigned int pFrequencyCount,
		unsigned int pFrequencyPatternLength){

	//The pattern is smaller than the Samplelength
	if(pFrequencyPatternLength>pSampleCount)
		return -1;

	unsigned int lSearchBlocks = ceil(pSampleCount/pFrequencyPatternLength);
	double lScore=0, lBestScore=0;
	int lSampleOffset=0,lBestOffset=0;
	int lBlockOffset=pFrequencyPatternLength/2;


	//go through pSamples in blocks of length pFrequencyPatternLength
	for(int i=lSearchBlocks-1;i>=0;i--){
		lSampleOffset=i*pFrequencyPatternLength;
		//the block would be larger than the existing Samples
		if(lSampleOffset+pFrequencyPatternLength>pSampleCount)
			lSampleOffset = pSampleCount - pFrequencyPatternLength;
		//the comperative base would be an amplitude of 100, which
		//will obviously never be reached
		lScore=findFrequencyPattern(pSamples+lSampleOffset,pFrequencyPatternLength,pFrequencies,pFrequencyCount,100);
		if(lScore>=lBestScore)
			lBestOffset=lSampleOffset;
	}

	//there is no signal at all, should not happen at all
	if(lBestScore<5)
		return 0;


	// search the beginning of the signal
	if(lSampleOffset-lBlockOffset<0)
		lSampleOffset=lBlockOffset-lSampleOffset;

	for(;lBlockOffset>0;lBlockOffset/=2){
		lScore=findFrequencyPattern(pSamples+lSampleOffset-lBlockOffset,
										pFrequencyPatternLength,pFrequencies,pFrequencyCount,100);
		if(lScore>=lBestScore)
			lSampleOffset -= lBlockOffset;
	}

	return lSampleOffset;
}







/***********************************************************************
 * normalizeSignal
 ***********************************************************************/
void CSignalToolkit::normalizeSignal(fftw_complex *pSignal, unsigned int pLength) {

	double max =  pSignal[0][0]; //abs(mAnalyzer.getAmplitudeOfComplex(mFftSyncSignalDataTimeDomain[0]));

	// Search for the highest value in the key signal
	for (unsigned int i=1; i<pLength; i++) {
		double value = fabs(pSignal[i][0]);//mAnalyzer.getAmplitudeOfComplex(mFftSyncSignalDataTimeDomain[i]));

		if (value > max)
			max = value;
	}

	// Calculate the divisor that we divide every amplitude by
	double divisor = max;

	// Multiply every amplitude so that the maximum value is exactly 1
	for (unsigned int i=0; i<pLength; i++)
		pSignal[i][0] = pSignal[i][0] / divisor ;
}


/***********************************************************************
 * generateSyncSignal
 ***********************************************************************/
void CSignalToolkit::generateSignal(double* pSamples, unsigned int pCount, unsigned int *pSyncFreq, unsigned int pSyncFreqCount) {

	// Allocate memory for IFFT
	fftw_complex *lFrequencyDomain;
	lFrequencyDomain = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * pCount);

	// Set the right amplitudes to 1
	float lBaseFrequency = (float)mSampleRate / (float)pCount;
	int index;

	// Calculate the FFT array index of the frequencies given above
	for (unsigned int i=0; i < (pSyncFreqCount) ; i++) {
		index = pSyncFreq[i] / lBaseFrequency;
		lFrequencyDomain[index         ][0] = 32.0;
		lFrequencyDomain[index         ][1] = 0.0;
		lFrequencyDomain[pCount - index][0] = 32.0;
		lFrequencyDomain[pCount - index][1] =  0.0;
	}

	// create a ifft plan
	fftw_plan lIFFT = fftw_plan_dft_1d(pCount, lFrequencyDomain, lFrequencyDomain, FFTW_BACKWARD, FFTW_ESTIMATE);

	// execute plan
	fftw_execute(lIFFT);


	normalizeSignal(lFrequencyDomain, pCount);

	for(unsigned int i=0;i<pCount;i++){
		pSamples[i] = lFrequencyDomain[i][0];
	}

	//free memory
	fftw_free(lFrequencyDomain);
	fftw_destroy_plan(lIFFT);
}



/***********************************************************************
 * crossCorrelation
 *
 * return 0 to infinitive. the lower the better
 ***********************************************************************/
double CSignalToolkit::crossCorrelationExact(double* pX, double* pY, unsigned int pCount){
	double lCorrelation = 0;

	for(unsigned int i=0;i<pCount;i++,pX++,pY++){
		if(*pX<*pY)
			lCorrelation+=*pY-*pX;
		else
			lCorrelation+=*pX-*pY;
	}
	return lCorrelation;
}


/**
 * Looks in the samples of *pSamples if the signal pPattern with a
 * Threshold of pThreshold is in it

 */
double CSignalToolkit::crossCorrelationBestMatch(
		double* pSamples,
		unsigned int pSampleCount,
		double* pPattern,
		unsigned int pPatternCount,
		double pThreshold){

	if(pSampleCount<pPatternCount)
		return -1;

	//both blocks are too large
	//make at least 20 difference
	if(pSampleCount-pPatternCount<20){
		pPatternCount-=(20-(pSampleCount-pPatternCount));
	}

	double result=0;
	double lSmallestValue=999;
	double lCorrelationValues[pSampleCount-pPatternCount+1];
	for(unsigned int i=0;i<pSampleCount-pPatternCount+1;i++){
		lCorrelationValues[i]=crossCorrelationExact(pPattern,pSamples+i,pPatternCount);
		if(lCorrelationValues[i]<lSmallestValue)
			lSmallestValue=lCorrelationValues[i];
	}
	return lSmallestValue;
}


void CSignalToolkit::setSampleRate(unsigned int pSampleRate) {
	if(pSampleRate>0)
		mSampleRate=pSampleRate;
}





double CSignalToolkit::getAmplitudeOfComplex(double* pComplex){

	if(pComplex==NULL)
		return 0;

	return sqrt(pow(pComplex[0],2) + pow(pComplex[1],2));
}


double CSignalToolkit::getPhaseOfComplex(double* pComplex){

	if(pComplex==NULL)
		return 0;

	return atan2(pComplex[0],pComplex[1]);
}
